#!/usr/bin/python
# -*- coding: UTF-8 -*-

import rospy  
from sensor_msgs.msg import Joy  
from geometry_msgs.msg import Twist  
from std_msgs.msg import String  

class Teleop:  
    def __init__(self):  

        rospy.init_node("Handle")  
        self.turn_scale = rospy.get_param("turn_scale",1)  
        self.drive_scale = rospy.get_param("drive_scale",1)  
        

        self.cmd = None  
        self.cmd_pub = rospy.Publisher("HANDLE_cmd_vel", Twist,queue_size=10)  
      
        rospy.Subscriber("Joystick_0", Joy, self.callback)  
        rate = rospy.Rate(20)  

        while not rospy.is_shutdown():  

            rate.sleep()  


    def callback(self, data):  

        """收joy 发twist """  
        self.cmd = Twist()  
        self.cmd.linear.x = data.axes[-2] * self.drive_scale
        self.cmd.linear.y =  - (data.axes[-1] * self.drive_scale)
        self.cmd.angular.z = data.axes[4] * self.turn_scale  
       
        self.cmd_pub.publish(self.cmd) 

 

if __name__ == "__main__": 
    try:
        rospy.loginfo("open!")
        Teleop()
    except rospy.ROSInterruptException:
        rospy.loginfo("INTERRUPT——ERROR")